/*
 * Copyright (c) 2021, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "ti_msp_dl_config.h"
#include <stdio.h>  // 添加stdio.h以使用sprintf
#include <stdlib.h> // 添加stdlib.h以使用abs函数
#include "OLED.h"
#include "MPU6050.h"
#include "MyI2C.h"
#include "MPU6050_Reg.h"
#include "Encoder.h"
#include "motor.h"
#include "pid.h"

int16_t MA, MB, MC, MD;//电机转速
int16_t tarMA, tarMB, tarMC, tarMD;         // 过渡目标值
int16_t tarMA_set, tarMB_set, tarMC_set, tarMD_set; // 期望目标速度
int16_t AccX, AccY, AccZ, GyroX, GyroY, GyroZ;//加速度计和陀螺仪数据
int8_t status = 0;//模式状态机，默认为模式0
int16_t sensor_status = 0;
int8_t SPEED_STEP = 1;  // 平滑度，默认步长
uint8_t acceleration_phase = 0;  // 加速阶段标志：0-初始化，1-加速中，2-已达目标
uint16_t acceleration_counter = 0;  // 加速计数器
int16_t real_target_speed[4] = {0, 0, 0, 0};  // 实际目标速度缓存

Attitude attitude = {0}; // 初始化为0

// 模式2的全局变量
uint8_t mode2_sub_state = 0;  // 子状态：0-初始化，1-直行到起点，2-循迹
uint16_t mode2_straight_timer = 0;  // 直行计时器

// 模式4的全局变量
int8_t circle_count = 0;   // 已完成的圈数
int8_t mode4_sub_state = 0;      // 子状态：0-初始化，1-直线循迹，2-检测拐角，3-转弯，4-完成转弯
int16_t start_angle = 0;   // 开始转弯时的角度
int16_t target_angle = 0;  // 目标转弯角度
uint16_t straight_count = 0; // 直线计数器
uint8_t corner_detected = 0;  // 拐角检测标志
uint8_t corner_count = 0;  // 拐角计数器


//模式1：直线行驶
void mode_1(void)
{
    // 设置目标速度，而不是直接设置实际速度
    tarMA_set = 80;
    tarMB_set = 80;
    tarMC_set = 80;
    tarMD_set = 80;
    
    //mode_1沿直线行驶，pid控制器已在定时器托管，mode1函数内设定目标值即可
    OLED_ShowNum(1, 9, status, 1);
    OLED_ShowNum(1, 1, MA, 3);
    OLED_ShowNum(2, 1, MB, 3);
    OLED_ShowNum(3, 1, MC, 3);
    OLED_ShowNum(4, 1, MD, 3);
    OLED_ShowNum(1, 5, tarMA, 3);
    OLED_ShowNum(2, 5, tarMB, 3);
    OLED_ShowNum(3, 5, tarMC, 3);
    OLED_ShowNum(4, 5, tarMD, 3);
}

//模式2：循迹
void mode_2(void)
{
    // 显示当前状态和电机速度
    OLED_ShowNum(1, 9, status, 1);
    OLED_ShowNum(1, 1, tarMA, 3);
    OLED_ShowNum(2, 1, tarMB, 3);
    OLED_ShowNum(3, 1, tarMC, 3);
    OLED_ShowNum(4, 1, tarMD, 3);

    // 显示目标速度
    OLED_ShowNum(1, 5, tarMA_set, 3);
    OLED_ShowNum(2, 5, tarMB_set, 3);
    OLED_ShowNum(3, 5, tarMC_set, 3);
    OLED_ShowNum(4, 5, tarMD_set, 3);
    
    // 显示传感器状态（二进制形式）
    for (int i = 0; i < 8; i++) {
        OLED_ShowChar(3, 9 + i, (sensor_status & (1 << (7-i))) ? '1' : '0');
    }
    
    // 显示子状态
    OLED_ShowNum(2, 9, mode2_sub_state, 1);
    
    // 状态机处理
    switch(mode2_sub_state) {
        case 0:  // 初始化
            // 设置直行速度为50
            tarMA_set = tarMB_set = tarMC_set = tarMD_set = 50;
            mode2_straight_timer = 0;
            mode2_sub_state = 1;  // 进入直行状态
            break;
            
        case 1:  // 直行到起点
            // 保持直行速度
            tarMA_set = tarMB_set = tarMC_set = tarMD_set = 50;
            
            // 计时器增加
            mode2_straight_timer++;
            
            // 当直行一段时间后，切换到循迹状态
            // 这里的200可以根据实际需要调整，表示直行的时间
            if (mode2_straight_timer > 200) {
                mode2_sub_state = 2;  // 进入循迹状态
            }
            break;
            
        case 2:  // 循迹状态
            // 在这个状态下，传感器逻辑和速度控制已经在定时器中断中处理
            // 这里不需要额外设置速度，只需要显示状态即可
            
            // 如果需要检测特定条件结束循迹，可以在这里添加
            break;
    }
}

//模式3：交叉循环
void mode_3(void)
{
    // MPU6050已经在定时器托管
    static int8_t turn_state = 0; // 0:初始状态 1:正在转弯 2:直行中
    static int16_t target_yaw = 0;
    
    OLED_ShowNum(1, 9, status, 1);
    OLED_ShowNum(1, 1, tarMA, 3);
    OLED_ShowNum(2, 1, tarMB, 3);
    OLED_ShowNum(3, 1, tarMC, 3);
    OLED_ShowNum(4, 1, tarMD, 3);
    
    // 根据转弯状态设置速度
    if (turn_state == 0) {
        // 初始化，准备左转45度
        NVIC_EnableIRQ(TIMNVIC_INST_INT_IRQN);
        NVIC_EnableIRQ(Sensor_INT_IRQN);
        target_yaw = attitude.yaw + 45; // 目标偏航角
        turn_state = 1; // 进入转弯状态
        
        // 左侧加速
        tarMB = 110;
        tarMD = 110;
        tarMC = 90;
        tarMA = 90;
    } else if (turn_state == 1) {
        // 检查是否达到目标角度
        if (fabsf(attitude.yaw - target_yaw) < 5) {
            // 转弯完成，进入直行状态
            tarMB = 100;
            tarMD = 100;
            tarMC = 100;
            tarMA = 100;
            turn_state = 2;
        }
    }
    
    // 显示当前偏航角和目标偏航角
    OLED_ShowNum(2, 9, (int)attitude.yaw, 3);
    OLED_ShowNum(3, 9, target_yaw, 3);
}

//模式4：绕四圈
void mode_4(void)
{
    // 显示当前状态
    OLED_ShowNum(1, 9, status, 1);
    OLED_ShowNum(1, 1, tarMA, 3);
    OLED_ShowNum(2, 1, tarMB, 3);
    OLED_ShowNum(3, 1, tarMC, 3);
    OLED_ShowNum(4, 1, tarMD, 3);
    
    // 显示当前圈数和子状态
    OLED_ShowNum(4, 9, circle_count, 1);
    OLED_ShowNum(4, 11, mode4_sub_state, 1);
    
    // 显示当前偏航角
    OLED_ShowNum(2, 9, (int)attitude.yaw, 3);
    
    // 状态机处理
    switch(mode4_sub_state) {
        case 0: // 初始化
            if (circle_count >= 4) {
                // 已完成4圈，重置状态并返回模式1
                circle_count = 0;
                mode4_sub_state = 0;
                status = 1;
                return;
            }
            
            // 初始化参数
            straight_count = 0;
            corner_detected = 0;
            mode4_sub_state = 1; // 进入直线循迹状态
            break;
            
        case 1: // 直线循迹
            // 使用传感器进行循迹，这部分已在定时器中断中处理
            
            // 计数器增加，用于确保小车已经走了一段直线
            straight_count++;
            
            // 检测是否已经走了足够长的直线，可以开始检测拐角
            if (straight_count > 100) { // 可根据实际情况调整阈值
                mode4_sub_state = 2; // 进入拐角检测状态
            }
            break;
            
        case 2: // 检测拐角
            // 检测拐角的条件：最左侧或最右侧传感器检测到线
            if (sensor_status & (1 << 0)) {
                // 检测到左拐角
                corner_detected = 1;
                start_angle = attitude.yaw;
                target_angle = start_angle - 90; // 左转90度
                mode4_sub_state = 3; // 进入转弯状态
            } else if (sensor_status & (1 << 7)) {
                // 检测到右拐角
                corner_detected = 2;
                start_angle = attitude.yaw;
                target_angle = start_angle + 90; // 右转90度
                mode4_sub_state = 3; // 进入转弯状态
            }
            
            // 如果一段时间内没有检测到拐角，可能需要特殊处理
            // 这里可以添加超时处理
            
            break;
            
        case 3: // 转弯
            if (corner_detected == 1) {
                // 左转
                tarMA = tarMC = 70;
                tarMB = tarMD = 30;
                
                // 检查是否已经转到目标角度
                if (fabsf(attitude.yaw - target_angle) < 10) {
                    mode4_sub_state = 4; // 转弯完成
                }
            } else if (corner_detected == 2) {
                // 右转
                tarMB = tarMD = 70;
                tarMA = tarMC = 30;
                
                // 检查是否已经转到目标角度
                if (fabsf(attitude.yaw - target_angle) < 10) {
                    mode4_sub_state = 4; // 转弯完成
                }
            }
            break;
            
        case 4: // 完成转弯
            // 重置为直线循迹速度
            tarMA = tarMB = tarMC = tarMD = 50;
            
            // 检测是否完成一圈（四个拐角）
            corner_count++;
            
            if (corner_count >= 4) {
                // 完成一圈
                circle_count++;
                corner_count = 0;
                mode4_sub_state = 0; // 回到初始化状态
            } else {
                // 继续下一段直线
                straight_count = 0;
                corner_detected = 0;
                mode4_sub_state = 1; // 回到直线循迹状态
            }
            break;
    }
    
    // 显示更多调试信息
    OLED_ShowNum(3, 11, corner_detected, 1);
    OLED_ShowNum(2, 11, (int)target_angle, 3);
}

//串口
void uart0_send_string(char* str)
{
    while(*str)
    {
        while(DL_UART_isBusy(UART_0_INST) == true);
        DL_UART_Main_transmitData(UART_0_INST, *str++);
    }
    // 等待最后一个字符发送完成
    while(DL_UART_isBusy(UART_0_INST) == true);
}



int main(void)
{
    char buffer[100];
    SYSCFG_DL_init();
    OLED_Init();
    PID_ALLinit();
    MPU6050_Init();
    //打开NCIV通道
    NVIC_EnableIRQ(Encoder_INT_IRQN);
    NVIC_EnableIRQ(TIMNVIC_INST_INT_IRQN);
    NVIC_EnableIRQ(Sensor_INT_IRQN);
    
    // 设置MPU6050为竖直安装模式，X轴朝上
    MPU6050_SetMountOrientation(MPU_MOUNT_VERTICAL_X_UP);
    // 显示校准信息
    OLED_ShowString(1, 3, "Calibrating...");
    Motor_SetSpeed(1,0);
    Motor_SetSpeed(2,0);
    Motor_SetSpeed(3,0);
    Motor_SetSpeed(4,0);

    // 进行传感器校准（采集100个样本）
    for(int i = 0; i < 100; i++)
    {
        MPU6050_GetData(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
        // 简单延时
        for(volatile int j = 0; j < 50000; j++);
    }
    // 显示校准完成
    OLED_Clear(); // 清屏
    status = 1;

    // 在 main 函数开始处添加简单的测试代码
    uart0_send_string("UART Test\r\n");
    for(volatile int j = 0; j < 1000000; j++); // 长延时

    while (1)
    {
        
        switch(status)
        {
           
            case 1:
                mode_1();
                break;
            case 2:
                mode_2();
                break;
            case 3:
                mode_3();
                break;
            case 4:
                mode_4();
                break;
        }

        
        sprintf(buffer, "%0.f,%0.f,%0.f\r\n",PID_GetTarget(1),PID_GetActual(1),PID_GetError(1));
        uart0_send_string(buffer);
        
        // 短暂延时，避免过快刷新
        for(volatile int j = 0; j < 10000; j++);
    }
}


//按键扫描函数
uint8_t KEY_SCAN(void)
{
    static uint8_t key_state = 0;
    static uint8_t key_count = 0;
    uint8_t current_state = DL_GPIO_readPins(KEY_PORT, KEY_control_PIN) == 0 ? 1 : 0;
    
    // 状态一致时计数增加
    if(current_state == key_state) {
        key_count++;
        if(key_count >= 4) {  // 连续4次状态一致才确认
            key_count = 0;
            return current_state;
        }
    } else {  // 状态不一致时重置
        key_state = current_state;
        key_count = 0;
    }
    
    return 0xFF;  // 返回0xFF表示按键状态不稳定
}

void handle_sensor_logic(void)
{
    static uint16_t last_valid_status = 0;  // 记录上一次有效的传感器状态
    static uint8_t lost_line_count = 0;     // 丢线计数器
    
    if((sensor_status & (1 << 3)) && (sensor_status & (1 << 4)))
    {
        // 中间两个传感器都检测到线，直行
        tarMA_set = tarMB_set = tarMC_set = tarMD_set = 50;
        last_valid_status = sensor_status;
        lost_line_count = 0;
    }
    else if(sensor_status & (1 << 0))
    {
        // 最左侧传感器检测到线，向左大转弯
        tarMA_set = tarMC_set = 70;
        tarMB_set = tarMD_set = 30;
        last_valid_status = sensor_status;
        lost_line_count = 0;
    }
    else if(sensor_status & (1 << 1))
    {
        // 左侧传感器检测到线，向左中转弯
        tarMA_set = tarMC_set = 60;
        tarMB_set = tarMD_set = 40;
        last_valid_status = sensor_status;
        lost_line_count = 0;
    }
    else if(sensor_status & (1 << 2))
    {
        // 左中传感器检测到线，向左小转弯
        tarMA_set = tarMC_set = 55;
        tarMB_set = tarMD_set = 45;
        last_valid_status = sensor_status;
        lost_line_count = 0;
    }
    else if(sensor_status & (1 << 5))
    {
        // 右中传感器检测到线，向右小转弯
        tarMB_set = tarMD_set = 55;
        tarMA_set = tarMC_set = 45;
        last_valid_status = sensor_status;
        lost_line_count = 0;
    }
    else if(sensor_status & (1 << 6))
    {
        // 右侧传感器检测到线，向右中转弯
        tarMB_set = tarMD_set = 60;
        tarMA_set = tarMC_set = 40;
        last_valid_status = sensor_status;
        lost_line_count = 0;
    }
    else if(sensor_status & (1 << 7))
    {
        // 最右侧传感器检测到线，向右大转弯
        tarMB_set = tarMD_set = 70;
        tarMA_set = tarMC_set = 30;
        last_valid_status = sensor_status;
        lost_line_count = 0;
    }
    else
    {
        // 没有传感器检测到线，丢线处理
        lost_line_count++;
        
        if (lost_line_count < 50) {  // 短时间丢线，继续使用上一次有效状态
            // 根据上一次有效状态增大转向角度，尝试找回线
            if (last_valid_status & (1 << 0)) {
                // 上次是最左侧，加大左转
                tarMA_set = tarMC_set = 80;
                tarMB_set = tarMD_set = 20;
            } else if (last_valid_status & (1 << 7)) {
                // 上次是最右侧，加大右转
                tarMB_set = tarMD_set = 80;
                tarMA_set = tarMC_set = 20;
            } else if (last_valid_status & ((1 << 1) | (1 << 2))) {
                // 上次是左侧，加大左转
                tarMA_set = tarMC_set = 70;
                tarMB_set = tarMD_set = 30;
            } else if (last_valid_status & ((1 << 5) | (1 << 6))) {
                // 上次是右侧，加大右转
                tarMB_set = tarMD_set = 70;
                tarMA_set = tarMC_set = 30;
            } else {
                // 上次是中间或无法判断，保持直行但降低速度
                tarMA_set = tarMB_set = tarMC_set = tarMD_set = 30;
            }
        } else {
            // 长时间丢线，降低速度直至停止
            if (lost_line_count > 100) {
                tarMA_set = tarMB_set = tarMC_set = tarMD_set = 0;
            } else {
                tarMA_set = tarMB_set = tarMC_set = tarMD_set = 20;
            }
        }
    }
}

void smooth_target_update(void)
{
    static uint16_t startup_counter = 0;
    
    // 检测是否需要进入加速阶段（从停止状态启动）
    if(acceleration_phase == 0 && 
       (tarMA_set != 0 || tarMB_set != 0 || tarMC_set != 0 || tarMD_set != 0) && 
       (tarMA == 0 && tarMB == 0 && tarMC == 0 && tarMD == 0)) {
        // 从停止状态启动，进入加速阶段
        acceleration_phase = 1;
        acceleration_counter = 0;
        // 保存实际目标速度
        real_target_speed[0] = tarMA_set;
        real_target_speed[1] = tarMB_set;
        real_target_speed[2] = tarMC_set;
        real_target_speed[3] = tarMD_set;
    }
    
    // 加速阶段处理
    if(acceleration_phase == 1) {
        acceleration_counter++;
        
        // 根据加速阶段计算当前目标速度（三段式加速）
        int16_t current_target_A, current_target_B, current_target_C, current_target_D;
        
        if(acceleration_counter < 100) {
            // 第一阶段：缓慢加速到25%
            float ratio = (float)acceleration_counter / 100.0f * 0.25f;
            current_target_A = (int16_t)(real_target_speed[0] * ratio);
            current_target_B = (int16_t)(real_target_speed[1] * ratio);
            current_target_C = (int16_t)(real_target_speed[2] * ratio);
            current_target_D = (int16_t)(real_target_speed[3] * ratio);
        } else if(acceleration_counter < 300) {
            // 第二阶段：线性加速到75%
            float ratio = 0.25f + (float)(acceleration_counter - 100) / 200.0f * 0.5f;
            current_target_A = (int16_t)(real_target_speed[0] * ratio);
            current_target_B = (int16_t)(real_target_speed[1] * ratio);
            current_target_C = (int16_t)(real_target_speed[2] * ratio);
            current_target_D = (int16_t)(real_target_speed[3] * ratio);
        } else if(acceleration_counter < 500) {
            // 第三阶段：缓慢加速到100%
            float ratio = 0.75f + (float)(acceleration_counter - 300) / 200.0f * 0.25f;
            current_target_A = (int16_t)(real_target_speed[0] * ratio);
            current_target_B = (int16_t)(real_target_speed[1] * ratio);
            current_target_C = (int16_t)(real_target_speed[2] * ratio);
            current_target_D = (int16_t)(real_target_speed[3] * ratio);
        } else {
            // 加速完成
            current_target_A = real_target_speed[0];
            current_target_B = real_target_speed[1];
            current_target_C = real_target_speed[2];
            current_target_D = real_target_speed[3];
            acceleration_phase = 2;  // 标记为已达目标
        }
        
        // 平滑更新到计算出的目标速度
        if(tarMA < current_target_A) tarMA += SPEED_STEP;
        else if(tarMA > current_target_A) tarMA -= SPEED_STEP;
        
        if(tarMB < current_target_B) tarMB += SPEED_STEP;
        else if(tarMB > current_target_B) tarMB -= SPEED_STEP;
        
        if(tarMC < current_target_C) tarMC += SPEED_STEP;
        else if(tarMC > current_target_C) tarMC -= SPEED_STEP;
        
        if(tarMD < current_target_D) tarMD += SPEED_STEP;
        else if(tarMD > current_target_D) tarMD -= SPEED_STEP;
    }
    // 正常运行阶段（非加速阶段或加速已完成）
    else {
        // 检测目标速度是否变化
        if(tarMA_set != real_target_speed[0] || 
           tarMB_set != real_target_speed[1] || 
           tarMC_set != real_target_speed[2] || 
           tarMD_set != real_target_speed[3]) {
            // 目标速度变化，更新缓存
            real_target_speed[0] = tarMA_set;
            real_target_speed[1] = tarMB_set;
            real_target_speed[2] = tarMC_set;
            real_target_speed[3] = tarMD_set;
        }
        
        // 常规平滑更新
        if(tarMA < tarMA_set) tarMA += SPEED_STEP;
        else if(tarMA > tarMA_set) tarMA -= SPEED_STEP;
        
        if(tarMB < tarMB_set) tarMB += SPEED_STEP;
        else if(tarMB > tarMB_set) tarMB -= SPEED_STEP;
        
        if(tarMC < tarMC_set) tarMC += SPEED_STEP;
        else if(tarMC > tarMC_set) tarMC -= SPEED_STEP;
        
        if(tarMD < tarMD_set) tarMD += SPEED_STEP;
        else if(tarMD > tarMD_set) tarMD -= SPEED_STEP;
    }
    
    // 检测是否停止
    if(tarMA_set == 0 && tarMB_set == 0 && tarMC_set == 0 && tarMD_set == 0) {
        // 目标速度为0，重置加速阶段
        acceleration_phase = 0;
    }
}


//pid控制受托函数
//100ms计算一次
//如需修改频率，需要进入内部修改公式
void Encoder_TimCaculate(void)
{
    static int16_t TimCaculate ;
    TimCaculate ++;
    if(TimCaculate == 100)
    {
        PID_SetTarget(1,tarMA);
        PID_SetTarget(2,tarMB);
        PID_SetTarget(3,tarMC);
        PID_SetTarget(4,tarMD);
        PID_Control(&MA,&MB,&MC,&MD);
        TimCaculate = 0;
    }
}
//MPU6050受托函数
//1ms计算一次
void MPU6050_TIM(void)
{
    // 读取传感器数据
    MPU6050_GetData(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
    // 更新姿态
    UpdateAttitude(AccX, AccY, AccZ, GyroX, GyroY, GyroZ, &attitude);  
}
//按键扫描受托函数
//20ms扫描一次
void KEY_tick(void)
{
    static uint8_t time = 0;
    static uint8_t curr_status = 0;
    static uint8_t last_status = 0;
    time++;
    if(time == 20)  // 降低扫描间隔，提高响应速度
    {
        last_status = curr_status;
        uint8_t scan_result = KEY_SCAN();
        
        // 只有当返回值有效时才更新状态
        if(scan_result != 0xFF) {
            curr_status = scan_result;
            
            // 检测按键按下（下降沿）
            if(curr_status == 1 && last_status == 0)
            {
                status++;
                if(status > 4)
                {
                    status = 1;
                }
            }
        }
        time = 0;
    }
}

//该中断为定时托管中断
void TIMNVIC_INST_IRQHandler(void)
{
    KEY_tick();
    
    // 在模式2、3、4中处理传感器逻辑
    if (status >= 2 && status <= 4)
    {
        // 主动读取传感器状态，不仅依赖中断
        sensor_status = 0; // 先清零
        
        // 采集传感器状态（低电平为有效）
        if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_0_PIN) == 0) sensor_status |= (1 << 0);
        if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_1_PIN) == 0) sensor_status |= (1 << 1);
        if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_2_PIN) == 0) sensor_status |= (1 << 2);
        if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_3_PIN) == 0) sensor_status |= (1 << 3);
        if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_4_PIN) == 0) sensor_status |= (1 << 4);
        if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_5_PIN) == 0) sensor_status |= (1 << 5);
        if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_6_PIN) == 0) sensor_status |= (1 << 6);
        if(DL_GPIO_readPins(Sensor_PORT, Sensor_PIN_7_PIN) == 0) sensor_status |= (1 << 7);
        
        handle_sensor_logic();   // 先根据传感器设置目标速度
        smooth_target_update();  // 再让实际目标速度逐步逼近
    }
    
    if (status != 0) 
    {
        Encoder_TimCaculate(); // pid调速
    }
    
    if(status == 4)
    {
        MPU6050_TIM(); // mode4内，MPU6050受托
    }
    
    DL_TimerA_clearInterruptStatus(TIMNVIC_INST, DL_TIMERA_INTERRUPT_ZERO_EVENT);
}





//该中断用来采样编码器脉冲
void GROUP1_IRQHandler (void)
{
     // 获取GPIOA中断状态 - 只获取我们关心的引脚
    uint32_t ma_a_status = DL_GPIO_getEnabledInterruptStatus(Encoder_MA_A_PORT, Encoder_MA_A_PIN);
    uint32_t mb_a_status = DL_GPIO_getEnabledInterruptStatus(Encoder_MB_A_PORT, Encoder_MB_A_PIN);
    uint32_t mc_a_status = DL_GPIO_getEnabledInterruptStatus(Encoder_MC_A_PORT, Encoder_MC_A_PIN);
    uint32_t md_a_status = DL_GPIO_getEnabledInterruptStatus(Encoder_MD_A_PORT, Encoder_MD_A_PIN);
    
    // 处理电机A的A相中断
    if (ma_a_status) {
        // 读取B相状态确定方向
        uint8_t direction = DL_GPIO_readPins(Encoder_MA_B_PORT, Encoder_MA_B_PIN) ? 1 : 0;
        // 更新电机A的计数器
        Encoder_Update(0, direction);
        // 清除中断状态
        DL_GPIO_clearInterruptStatus(GPIOA, Encoder_MA_A_PIN);
    }
    
    // 处理电机B的A相中断
    if (mb_a_status) {
        // 读取B相状态确定方向
        uint8_t direction = DL_GPIO_readPins(Encoder_MB_B_PORT, Encoder_MB_B_PIN) ? 1 : 0;
        // 更新电机B的计数器
        Encoder_Update(1, direction);
        // 清除中断状态
        DL_GPIO_clearInterruptStatus(GPIOA, Encoder_MB_A_PIN);
    }
    
    // 处理电机C的A相中断
    if (mc_a_status) {
        // 读取B相状态确定方向
        uint8_t direction = DL_GPIO_readPins(Encoder_MC_B_PORT, Encoder_MC_B_PIN) ? 1 : 0;
        // 更新电机C的计数器
        Encoder_Update(2, direction);
        // 清除中断状态
        DL_GPIO_clearInterruptStatus(GPIOA, Encoder_MC_A_PIN);
    }
    
    // 处理电机D的A相中断
    if (md_a_status) {
        // 读取B相状态确定方向
        uint8_t direction = DL_GPIO_readPins(Encoder_MD_B_PORT, Encoder_MD_B_PIN) ? 1 : 0;
        // 更新电机D的计数器
        Encoder_Update(3, direction);
        // 清除中断状态
        DL_GPIO_clearInterruptStatus(Encoder_MD_A_PORT, Encoder_MD_A_PIN);
    }
}

